//===========================================================================
/*
  CS277 - Experimental Haptics
  Winter 2010, Stanford University

  You may use this program as a boilerplate for starting your homework
  assignments.  Use CMake (www.cmake.org) on the CMakeLists.txt file to
  generate project files for the development tool of your choice.  The
  CHAI3D library directory (chai3d-2.1.0) should be installed as a sibling
  directory to the one containing this project.

  These files are meant to be helpful should you encounter difficulties
  setting up a working CHAI3D project.  However, you are not required to
  use them for your homework -- you may start from anywhere you'd like.

  \author    Francois Conti & Sonny Chan
  \date      January 2010
*/
//===========================================================================

//---------------------------------------------------------------------------
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
//---------------------------------------------------------------------------
#include <GL/glew.h>
#include "chai3d.h"

//---------------------------------------------------------------------------

#include "ShowCase.hpp"

//---------------------------------------------------------------------------
// DECLARED CONSTANTS
//---------------------------------------------------------------------------

// initial size (width/height) in pixels of the display window
const int WINDOW_SIZE_W         = 600;
const int WINDOW_SIZE_H         = 600;

// mouse menu options (right button)
const int OPTION_FULLSCREEN     = 1;
const int OPTION_WINDOWDISPLAY  = 2;

// maximum number of haptic devices supported in this demo
const int MAX_DEVICES           = 8;


//---------------------------------------------------------------------------
// DECLARED VARIABLES
//---------------------------------------------------------------------------

// a world that contains all objects of the virtual environment
cWorld* world;

// a camera that renders the world in a window display
cCamera* camera;

// a light source to illuminate the objects in the virtual scene
cLight *light;

// width and height of the current window display
int displayW  = 0;
int displayH  = 0;

// a haptic device handler
cHapticDeviceHandler* handler;

// a pointer to the first haptic device detected on this computer
cGenericHapticDevice* hapticDevice = 0;

// a 3D cursor for the haptic device
cShapeSphere* cursor = 0;

// a line to display velocity of the haptic interface
cShapeLine* velocityVector;

// labels to show haptic device position and update rate
cLabel* positionLabel;
cLabel* rateLabel;
double rateEstimate = 0;

cPrecisionClock gclock;
int gcounter;
int grateEstimate;

// material properties used to render the color of the cursors
cMaterial matCursorButtonON;
cMaterial matCursorButtonOFF;

// status of the main simulation haptics loop
bool simulationRunning = false;

// has exited haptics simulation thread
bool simulationFinished = false;

//---------------------------------------------------------------------------
// DECLARED FUNCTIONS DECLARATION
//---------------------------------------------------------------------------

// callback when the window display is resized
void resizeWindow(int w, int h);

// callback when a keyboard key is pressed
void keySelect(unsigned char key, int x, int y);

// callback when the right mouse button is pressed to select a menu item
void menuSelect(int value);

// function called before exiting the application
void close(void);

// main graphics callback
void updateGraphics(void);

// main haptics loop
void updateHaptics(void);


void initWorld(){
 //-----------------------------------------------------------------------
  // 3D - SCENEGRAPH
  //-----------------------------------------------------------------------
  // create a new world.
  world = new cWorld();

  // set the background color of the environment
  // the color is defined by its (R,G,B) components.
  world->setBackgroundColor(0.2, 0.2, 0.2);
  
  // create a camera and insert it into the virtual world
  camera = new cCamera(world);
  world->addChild(camera);

  // position and oriente the camera
  /*camera->set( cVector3d (0.2, 0.0, 0.0),    // camera position (eye)
	       cVector3d (0.0, 0.0, 0.0),    // lookat position (target)
	       cVector3d (0.0, 0.0, 1.0));   // direction of the "up" vector*/

  // set the near and far clipping planes of the camera
  // anything in front/behind these clipping planes will not be rendered
  camera->setClippingPlanes(0.01, 10.0);

  // create a light source and attach it to the camera
  light = new cLight(world);
  camera->addChild(light);                   // attach light to camera
  light->setEnabled(true);                   // enable light source
  light->setPos(cVector3d( 2.0, 0.5, 1.0));  // position the light source
  light->setDir(cVector3d(-2.0, 0.5, 1.0));  // define the direction of the light beam

  // create a label that shows the haptic loop update rate
  rateLabel = new cLabel();
  rateLabel->setPos(8, 24, 0);
  camera->m_front_2Dscene.addChild(rateLabel);
}

void initGL(int argc, char* argv[]){
  //-----------------------------------------------------------------------
  // OPEN GL - WINDOW DISPLAY
  //-----------------------------------------------------------------------

  // initialize GLUT
  glutInit(&argc, argv);

  // retrieve the resolution of the computer display and estimate the position
  // of the GLUT window so that it is located at the center of the screen
  int screenW = glutGet(GLUT_SCREEN_WIDTH);
  int screenH = glutGet(GLUT_SCREEN_HEIGHT);
  int windowPosX = (screenW - WINDOW_SIZE_W) / 2;
  int windowPosY = (screenH - WINDOW_SIZE_H) / 2;

  // initialize the OpenGL GLUT window
  glutInitWindowPosition(windowPosX, windowPosY);
  glutInitWindowSize(WINDOW_SIZE_W, WINDOW_SIZE_H);
  glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE);
  glutCreateWindow(argv[0]);
  glutDisplayFunc(updateGraphics);
  glutKeyboardFunc(keySelect);
  glutReshapeFunc(resizeWindow);
  glutSetWindowTitle("HAPTICUDA Showcase");

  glewInit();
  if (!glewIsSupported("GL_VERSION_2_0 GL_VERSION_1_5 GL_ARB_multitexture GL_ARB_vertex_buffer_object")) {
    fprintf(stderr, "Required OpenGL extensions missing.");
    exit(-1);
  }
  glEnable(GL_DEPTH_TEST);
  glClearColor(0.25, 0.25, 0.25, 1.0);

}

void initHaptics(){
  //-----------------------------------------------------------------------
  // HAPTIC DEVICES / TOOLS
  //-----------------------------------------------------------------------

  // create a haptic device handler
  handler = new cHapticDeviceHandler();

  // read the number of haptic devices currently connected to the computer
  int numHapticDevices = handler->getNumDevices();

  // if there is at least one haptic device detected...
  if (numHapticDevices)
    {
      // get a handle to the first haptic device
      handler->getDevice(hapticDevice);

      // open connection to haptic device
      hapticDevice->open();

      // initialize haptic device
      hapticDevice->initialize();

      // retrieve information about the current haptic device
      cHapticDeviceInfo info = hapticDevice->getSpecifications();

      // create a cursor with its radius set
      cursor = new cShapeSphere(0.01);

      // add cursor to the world
      world->addChild(cursor);

      // create a small line to illustrate velocity
      velocityVector = new cShapeLine(cVector3d(0,0,0), cVector3d(0,0,0));

      // add line to the world
      world->addChild(velocityVector);

      positionLabel = new cLabel();
      positionLabel->setPos(8, 8, 0);
      camera->m_front_2Dscene.addChild(positionLabel);
    }

  // here we define the material properties of the cursor when the
  // user button of the device end-effector is engaged (ON) or released (OFF)
  // a light orange material color
  matCursorButtonOFF.m_ambient.set(0.5, 0.2, 0.0);
  matCursorButtonOFF.m_diffuse.set(1.0, 0.5, 0.0);
  matCursorButtonOFF.m_specular.set(1.0, 1.0, 1.0);
  // a blue material color
  matCursorButtonON.m_ambient.set(0.1, 0.1, 0.4);
  matCursorButtonON.m_diffuse.set(0.3, 0.3, 0.8);
  matCursorButtonON.m_specular.set(1.0, 1.0, 1.0);
}


int main(int argc, char* argv[])
{
  //-----------------------------------------------------------------------
  // INITIALIZATION
  //-----------------------------------------------------------------------

  printf ("\n");
  printf ("-----------------------------------\n");
  printf ("CS277 - Experimental Haptics\n");
  printf ("Homework Boilerplate Application\n");
  printf ("January 2010, Stanford University\n");
  printf ("-----------------------------------\n");
  printf ("\n\n");


  initWorld();
  initHaptics();
  initGL(argc, argv);
  
  StartParticle();

  //-----------------------------------------------------------------------
  // START SIMULATION
  //-----------------------------------------------------------------------
  // simulation in now running
  simulationRunning = true;

  // create a thread which starts the main haptics rendering loop
  cThread* hapticsThread = new cThread();
  hapticsThread->set(updateHaptics, CHAI_THREAD_PRIORITY_HAPTICS);


  //graphics clock 
  gclock.setTimeoutPeriodSeconds(1.0);
  gclock.start(true);
  gcounter = 0;





  // start the main graphics rendering loop
  glutMainLoop();

  // close everything
  close();
  // exit
  return (0);
}

//---------------------------------------------------------------------------

void resizeWindow(int w, int h)
{
  // update the size of the viewport
  displayW = w;
  displayH = h;
  glViewport(0, 0, displayW, displayH);
}

//---------------------------------------------------------------------------

void keySelect(unsigned char key, int x, int y)
{
  // escape key
  if ((key == 27) || (key == 'x'))
    {
      // close everything
      close();

      // exit application
      exit(0);
    }
}

//---------------------------------------------------------------------------

void menuSelect(int value)
{
  switch (value)
    {
      // enable full screen display
    case OPTION_FULLSCREEN:
      glutFullScreen();
      break;

      // reshape window to original size
    case OPTION_WINDOWDISPLAY:
      glutReshapeWindow(WINDOW_SIZE_W, WINDOW_SIZE_H);
      break;
    }
}

//---------------------------------------------------------------------------

void close(void)
{
  // stop the simulation
  simulationRunning = false;

  // wait for graphics and haptics loops to terminate
  while (!simulationFinished) { cSleepMs(100); }

  // close the haptic devices
  if (hapticDevice)
    {
      hapticDevice->close();
    }
}

//---------------------------------------------------------------------------

void updateGraphics(void)
{

  StepParticle();

  
  // update the label showing the position of the haptic device
  if (cursor)
    {
      cVector3d position = cursor->getPos() * 1000.0; // convert to mm
      char buffer[128];
      sprintf(buffer, "device position: (%.2lf, %.2lf, %.2lf) mm",
	      position.x, position.y, position.z);
      positionLabel->m_string = buffer;
    }

  // update the label with the haptic refresh rate
  char buffer[128];
  sprintf(buffer, "haptic rate: %.0lf Hz\tGraphics rate: %d", rateEstimate,grateEstimate);
  rateLabel->m_string = buffer;

  // render world
  camera->renderView(displayW, displayH);

  // Swap buffers
  glutSwapBuffers();

  // check for any OpenGL errors
  GLenum err;
  err = glGetError();
  if (err != GL_NO_ERROR) printf("Error:  %s\n", gluErrorString(err));

  // inform the GLUT window to call updateGraphics again (next frame)
  if (simulationRunning)
    {
      glutPostRedisplay();
    }
    
  ++gcounter;
  if (gclock.timeoutOccurred()) {
    gclock.stop();
    grateEstimate = gcounter;
    gcounter = 0;
    gclock.start(true);
  }
}

//---------------------------------------------------------------------------

void updateHaptics(void)
{
  // a clock to estimate the haptic simulation loop update rate
  cPrecisionClock pclock;
  pclock.setTimeoutPeriodSeconds(1.0);
  pclock.start(true);
  int counter = 0;

  // main haptic simulation loop
  while(simulationRunning)
    {
      if (!hapticDevice) continue;
		
      // read position of haptic device
      cVector3d newPosition;
      hapticDevice->getPosition(newPosition);

      // update position and orientation of cursor
      cursor->setPos(newPosition);

      // read linear velocity from device
      cVector3d linearVelocity;
      hapticDevice->getLinearVelocity(linearVelocity);

      // update the line showing velocity
      velocityVector->m_pointA = newPosition;
      velocityVector->m_pointB = newPosition + linearVelocity;

      // read user button status
      bool buttonStatus;
      hapticDevice->getUserSwitch(0, buttonStatus);

      // adjust the color of the cursor according to the status of
      // the user switch (ON = TRUE / OFF = FALSE)
      cursor->m_material = buttonStatus ? matCursorButtonON : matCursorButtonOFF;

      // compute a reaction force (a spring that pulls the device to the origin)
      const double stiffness = 100.0; // [N/m]
      cVector3d force;
      force.zero();

      // send computed force to haptic device
      hapticDevice->setForce(force);

      // estimate the refresh rate
      ++counter;
      if (pclock.timeoutOccurred()) {
	pclock.stop();
	rateEstimate = counter;
	counter = 0;
	pclock.start(true);
      }
    }
    
  // exit haptics thread
  simulationFinished = true;
}














